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TECHNICAL MEMORANDUM 


INVERTING THE PENDULUM USING FUZZY CONTROL 

BACKGROUND 


This work was done as the result of a desire to demonstrate fuzzy motor control with an empha- 
sis on doing something that is difficult to do by conventional control methods. The demonstration was 
conducted in hardware rather than as a software simulation. The Center Director’s Discretionary Fund 
(CDDF) was the avenue chosen for funding the task. A CDDF typically runs for 2 years; the funding 
commitment is $20,000 to $40,000. This work eventually stretched to 4 years. The project was not as 
extensive as originally intended, primarily because CDDF work is given a low priority. A highly nonlin- 
ear system was chosen as the best candidate for the demonstration because fuzzy control is inherently 
nonlinear and conventional controllers generally only work well with linear systems. 

The most common nonlinear system for demonstrating a control strategy is perhaps the inverted 
pendulum. The version chosen for this demonstration is on a circular base rather than the usual track. 
This way, it cannot run off the track. Also, while the act of maintaining the pendulum in an inverted 
position is somewhat nonlinear, the act of bringing the pendulum to a vertical position from the hanging 
position is extremely nonlinear. The fuzzy controller does this, as well as maintaining the pendulum 
inverted, and no one could define another controller that could invert the pendulum at all. 

After getting the controller to work with the single pendulum, the next task was to try to make 
the controller work with a double inverted pendulum. This has worked in simulation to a limited extent, 
but not well enough to try in hardware. After that, a high-frame-rate camera was purchased in order to 
attempt real-time control of the pendulum using only visual inputs. That is, image processing is done in 
real time to determine the pendulum angle, pendulum rate, and base rate (and base position, if desired). 
It may be possible to use conventional cameras with the high-frame-rate camera for stereo vision. 

Although time for the CDDF work has been exhausted, some work may continue after the report 
is published. Two other projects were ready to be used for fuzzy control if time had been available. One 
involved a control-structures interaction suitcase demonstration that uses a piezoelectric damper to 
reduce structural vibration. The other was an electromechanical actuator with a conventional controller. 



FUZZY CONTROL 


Fuzzy controllers typically have more sensors than conventional controllers, while precision is 
generally not as high as with other controllers. However, due to the ease with which the control can be 
changed and its total nonlinear capability, fuzzy control often works better and can be built more quickly 
than anything else. Since there are so many parameters, there may be a need to use something like 
genetic algorithms to optimize everything, but, even without optimization, reasonable results can be 
obtained. Figure 1 is a block diagram of the inverted pendulum system. The system includes three inputs 
into the pendulum and one output into the motor. 



Figure 1. — Inverted pendulum control system block diagram. 

The sequence inside the controller begins with sending the inputs through an analog-to-digital 
(A/D) converter. These digitized inputs are each placed (fuzzified) into one of typically three, five, or 
seven ranges; rules are written with these ranges as arguments rather than with the individual value as 
arguments. Then the rules are evaluated and fuzzy values are produced for each one using the max-min 
method, max -dot (used here, also called max-product), or other inference method. In rule evaluation, 
“or” implies selecting the maximum of the two values, while “and” selects the minimum of the two 
values. In either case, the value itself is at the intersection of the membership function and the actual 
input number. With max-dot, the output membership function is scaled to the result of the minimum 
value or maximum value, whereas with the max-min it is clipped to that value. Both methods give 
similar results. These rule outputs are then combined (defuzzified) by the centroid (used here), height, or 
other method to produce one numerical (crisp) value. The combination of all the rule outputs generally 
produces an irregularly shaped result. The centroid of this irregular shape (easily computed for trapezoi- 
dal membership functions) is the crisp output value. In u4e height method, just the height of each rule 
output is used and the weighted output of all contributions is used. However, in the height method, 
unsymmetrical output membership functions cause errors. Finally, a digital-to-analog (D/A) converter is 
used to produce the voltage needed to drive the actuator. 












PENDULUM HARDWARE 


The pendulum hardware was built on a large inverted trash can. A line drawing of the hardware 
is shown in figure 2, and a photograph is shown in figure 3. The motor in the center rotates the bar with 
the actual pendulum attached to one end and the counterweight at the other. A tachometer is geared to 
the motor for one of the three inputs. A resolver is geared to the pendulum to measure the position of the 
pendulum as the second input, and successive resolver values are compared to produce pendulum rate as 
the third input. The command goes to the motor for the systems’ only output. A motor position measure- 
ment would be desirable, but the only method for obtaining that, as built, is to integrate the tachometer 
output. This is only an 8-bit value and is not accurate enough to be useful. A future redesign could 
provide improvement on this part. 



Figure 2. — Line drawing of the pendulum hardware. 
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Figure 3. — Photograph of the single pendulum hardware. 


The electronic hardware uses a 12-bit resolver/digital (R/D) converter for the pendulum input, an 
8-bit converter (in the Motorola 6811 microprocessor) for the motor tachometer, and a 12-bit (uses 8 of 
the 12) D/A converter for the motor drive. The Motorola processor is a model 68HC11E8, which has 2 
kilobytes of electrically erasable programmable read-only memory (EEPROM), 256 bytes RAM, and 4 
A/D converters (8-bit). Figure 4 shows a photograph of this hardware, where theR/D and A/D converters 
are on the left side and the 6811 microprocessor is on the right side. There are three power supplies: 20 
V at 3A for the motor, -20 V at 3A for the motor, and 5 V for the signals and central processing unit. 
Figure 5 shows the two large power supplies above the bench and the 5-V supply on the bench to the 
right. The motor is rated for 0.5 foot-pound at 5 A. The motor and tachometer are most visible in 
figure 3. 
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Figure 4. — Controller electronics with 6811 microprocessor on right 



Figure 5. — Total system hardware. 





SOFTWARE 


Simulation was done first. Matlab software was programmed to derive the dynamic equations 
using the Lagrange method. This was done using the symbolic math toolbox. The Matlab code is shown 
in appendix A. The kinetic energy equation is k, the potential equation is p, the dissipation equation is d, 
and the forcing function is ql. Xd2 and xd4 are the two resulting differential equations describing the 
system dynamics. These are shown as xdot(2) and xdot(4) in the function xdot. Then Matlab was used to 
simulate these equations with numbers approximating actual hardware values. A small amount of tuning 
was done in order to set gains, check scale factors, etc. The controller was then built and simulated with 
the Togai fuzzy control package with the dynamic equations taken from Matlab. 

The membership functions developed for the system were worked out with the Togai software 
and are shown in appendix B. There are four for the single pendulum: Theta for pendulum angle, dTheta 
for pendulum rate, Mrate for the motor rate, and Motor for the motor command. These are usually 
triangular, although trapezoidal is also common. An optimizing algorithm could determine that some 
other shape would work better, that different widths would be better, or perhaps that a different number 
of functions would be better. “Negative small,” for example, refers to any data point within that particu- 
lar membership function. 

The real-time fuzzy controller software that actually ran in the 6811 microprocessor was gener- 
ated by Togai in 6811 assembler language. The rest of the software to do sensor inputs, use the controller 
inputs and output, and do scaling, etc., was written by hand; all the software was combined and as- 
sembled by the Avocet assembler on a standard personal computer. Appendix C has the Togai-generated 
code for the membership functions, rules, and variables. It also has its simulation initialization code and 
then the simulation code itself. This is followed by the Avocet linker code and the resulting mapping. 
This was downloaded to the 6811 microprocessor via a serial link and then run. The object code for this 
is shown in ASCII format as sent to the 6811 microprocessor. Finally, the handwritten assembly code is 
shown which simply calls the Togai code as a subroutine to give the fuzzy controller output that results 
from the present inputs. This handwritten code also handles timers, scaling, limiting, and input/output. 
Procom was used to send the ASCII to the 6811 microprocessor. The ASCII routines at the end of the 
handwritten code were used to send data back to the computer for troubleshooting. 

The control (rule) matrix in the fuzzy controller is full, i.e., there are no empty combinations. 

This usually means (and does here) that the rules have not been optimized (number of rules minimized). 
The complete control matrix is shown in figure 6. The theta-thetadot portion of the matrix is the standard 
pendulum controller used to keep the pendulum upright and stable once it is upright. The motor rate- 
theta portion keeps the motor rate to a minimum. The two shaded columns are all that is needed to invert 
the pendulum. The control matrix can be read, for example, by saying that if theta is negative small and 
thetadot is positive small, then the control is zero. 
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Figure 6. — Single pendulum complete control matrix. 


An output results from evaluating all the rules and combining their contributions. If, for example, 
the theta value is -20 and the thetadot value is +6, both determined from the actual sensor readings 
scaled from -128 to +127, moving to the membership function intersections will give a value of mem- 
bership for each and will show which rules apply. If a rule has positive large and no sensor value is 
positive large, then that rule does not apply. However, for -20 and +6, some rule(s) will apply. If none 
do, as could occur after optimization was done, no output change occurs. One way of using these rule 
results to determine the actual value for the output is to combine them using a center-of-gravity method 
(explained earlier). This output is what gets sent to the D/A converter for the motor. Fuzzy control is a 
good method for combining sometimes conflicting rules to produce a useful output and is an inherent 
method of doing smooth, nonlinear gain switching. 
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RESULTS 


A VHS tape of the controller in action is available. Starting the pendulum in the already inverted 
position results in the pendulum’s remaining inverted, but with some wavering and some slow rotation 
of the pendulum around the can. The pendulum must move (start to fall) before the controller has an 
error to correct. This movement was left somewhat exaggerated to illustrate visually how the control 
operates. The motor must also move the support bar under the pendulum to maintain control, and this 
causes the bar to wander around the can since the can is not very sturdy and the bar falls different 
amounts before the controller gets under it. If motor position (angle) was available, the controller would 
be able to limit the wander to a relatively narrow range about a fixed angle. 

Starting the pendulum in the down position results in somewhat violent swinging as the control- 
ler begins to upright the pendulum. This control is extremely nonlinear and actually unstable until a 
nearly inverted position is reached. The pendulum can be jarred such that it falls, but it always rights 
itself. It can be prodded below the point of losing control, and it is quite robust. When control is lost, the 
pendulum rights itself again. 

Figure 7 shows the single pendulum starting in its rest position. The darker line is Mrate while 
Theta is the one that settles in at about 3.3 sec. The third line is Mpos. The violent swinging is evident 
first, followed by stabilizing upright to the limit cycles of position and rate. Figure 8 shows a transient 
response of the double pendulum stabilizing to the inverted position. The lighter line is the lower bar 
position while the darker line is the upper bar position. The stable angle range is quite small, indicating 
either that the system is hard to control or that the controller is not optimized for this particular system. 



Figure 7. — Single pendulum transient response. 
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Figure 8. — Double pendulum transient response. 


9 




CONCLUSIONS 


Although one objective of this work was to compare this controller with more conventional 
controllers, no other controller that would invert the pendulum could be defined. There was speculation 
that a piecewise linear controller with much switching could be made to work, but it was never pro- 
duced. In fact, the f uzz y controller is the ultimate switching controller since it transitions smoothly 
between the various rules (pieces) to accomplish the control. Although keeping the pendulum inverted is 
not difficult for even a set-point controller, inverting it is where fuzzy control excels. 
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FUTURE WORK 


Balancing the double pendulum has been done here in simulation but not in hardware. The 
simulation was not optimized for this, and the angle ranges for stability were only a few degrees. It is 
not certain if the present hardware would even allow the double pendulum to be inverted. The hardware 
may not be sturdy enough to make it work and, in fact, it may not even be theoretically possible to do. 
More work could be done here. 

There is a suitcase demonstration of a control-structure interaction device that uses piezoelectric 
material to provide damping to a thin, flexible beam (about 10 inches long). The fuzzy control could be 
tested to determine how it works just by controlling the motor. Perhaps the piezo material could also be 
tied to an output for additional control. An earlier simulation of fuzzy control as applied to this device 
was made by a summer faculty employee and appeared to work well, but it was never tested on the 
hardware. A 6811 microprocessor had been added to the hardware and it was ready to be used. 

An electromechanical actuator was to be controlled with a fuzzy controller and compared with 
the conventional controllers presently being used. There did not seem to be much improvement possible 
here since the controller was full on most of the time. Only when near the commanded point would 
some improvement be possible. Perhaps some worthwhile improvement could be made here. 

A high-frame-rate camera (and frame-grabber board) was purchased to derive all the pendulum 
control inputs from image processing, thereby using no sensors (or wiring) on the pendulum itself. This 
particular implementation of the pendulum, however, with its rotation, presents various perspectives to 
any fixed camera position and this has to be resolved. Two standard video cameras were also purchased, 
and one or both of these could possibly substitute or be used with the high-frame-rate camera. It may be 
possible to use two cameras to solve the rotational perspective problem. But time did not permit any of 
this to be done. 

This project received much attention and demonstrated that fuzzy control is a useful tool for the 
engineer when appropriate. 
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APPENDIX A 
MATLAB CODE 1 

PENDULUM. M. 


clear;clc 

k= ’ l/2*mpl *((rbar*Dg-len/2 *cos(th)* Dth) A 2+(len/2*sin(th)*Dth) A 2)+l/2*ibar*Dg A 2+l/2*ipl*Dth A 2’; 
pretty(k) 

p=’mpl*gr*len/2*cos(th)’; %gr=gravity 
pretty (p) 

d=’l/2*dpl*Dth A 2+l/2*dbar*Dg A 2'; 

pretty (d) 

ql=’tm/rbar’; 

q2=’0'; 

pretty(ql) 

pretty(q2) 

g=’g(t)’; 

Dg=diff(g,’t’); 

th=’th(t)’; 

Dth=diff(th,’t’); 

kl=diff(k,’Dth’); 

kl=subs(kl,th,’th’); 

kl=subs(kl,Dth,’Dth’); 

kl=subs(kl,Dg,’Dg’); 


k2=diff(kl,’t’); 

k2=subs(k2,’D2g’,’diff(diff(g(t),t),t)’); 
k2=subs(k2,’D2th ’ , ’diff(diff(th(t),t),t) ’); 
k2=subs(k2, ’Dg ’ , ’ diff(g(t),t) ’); 
k2=subs(k2,’Dth’,’diff(th(t),t) ’); 
k2=subs(k2, ’ th ’ , ’ th( t ’ ) ; 


k3=diff(k,’th’); 

k4=diff(p,’th’); 

k5=diff(d,’Dth’); 
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k6=diff(k,’Dg’); 
k6=subs(k6,th,’th’); 
k6=subs(k6,Dth, ’ Dth ’); 
k6=subs(k6,Dg, ’ Dg ’); 

k7=diff(k6,’t’); 

k7=subs(k7,’D2g’, , diff(diff(g(t),t),t)’); 
k7=subs(k7,’D2th’,’diff(diff(th(t),t),t)’); 
k7=subs(k7, ’ Dg ’ , ’diff(g(t),t) ’ ); 
k7 =subs(k7, ’ Dth diff(th(t),t) ’ ) ; 
k7=subs(k7,’th’,’th(t)’); 


k8=diff(k,’g’); 


k9=diff(p,’g’); 


klO=diff(d,’Dg’); 

R=symop(k2, ,q2, ,k3, ’ + \k4, ’ + ’ ,k5); 

simplify(R); 

%pretty(R) 

S=symop(k7, ,q 1 , ' ’-\k8, ’ + ’ ,k9, ’ ■ + ’ ,kl 0); 

simplify(S); 

%pretty(S) 

[gl,g2]=solve(R,S,’D2th,D2g’); 

%pretty(gl) 

%pretty(g2) 

R=subs(R,.4,’mpr); 

S=subs(S,.4,’mpl’); 

R=subs(R,.005,’ipl ’); 
S=subs(S,.005,’ipl’); 

R=subs(R,9.8,’gr’); 

S=subs(S,9.8,’gr’); 

R=subs(R,.08, ’ ibar ’); 
S=subs(S,.08,’ibar’); 
R=subs(R,.01,’dbar’); 
S=subs(S,.01,’dbar’); 

R=subs(R,.01,’dpl ’); 

S=subs(S, .0 1 , ’dp 1’ ); 

R=subs(R,.2,’len’); 

S=subs(S,.2,’len’); 

R=subs(R,.2,’rbar’); 

S=subs(S,.2,’rbar’); 
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R=subs(R,0,’tm’); 

S=subs(S,0,’tm’); 

[gl,g2]=soIve(R,S,’D2g,D2th’); 

gl=subs(gl, , b’,’g’); 

gl=subs(gl/c’,’Dg’); 

gl=subs(gl,’d’, , th’); 

g=subs(gl,’e’,’Dth’); 

g2=subs(g2, ’b g ’); 

g2=subs(g2,’c , ,’Dg’); 

g2=subs(g2,’d’,’th’); 

h=subs(g2,’e’,’Dth’); 

g=simple(g); 

h=simple(h); 

g=collect(g) 

h=collect(h) 

pretty(g) 

pretty(h) 

g3=’Dc=g’; 

hl=’De=h’; 

g3=subs(g3,g,’g’) 

hl=subs(hl,h,’h’) 

%[b,c,d,e]=dsolve(‘Db=c’,g3, , Dd=e’,hl,’b(0)=.0r, , c(0)=0’, , d(0)=0 , ,’e(0)=0 , ,’t’) 

xd2=subs(g,’x(l)\’b’); 
xd2=subs(xd2, ’x(2) ’ , ’ c ’ ) ; 
xd2=subs(xd2,’x(3) , ,’d’); 

xd4=subs(h, ’x( 1 ) ’ , ’b ’ ) ; 

xd4=subs(xd4,’x(2)’,’c’); 

xd4=subs(xd4,’x(3)Yd’); 

xd2=subs(xd2, ’x(4) ’ , ’e ’ ) 
xd4=subs(xd4,’x(4)’,’e’) 

t0=0;tf=3; 

x0=[0 0 .01 0]’; %initial conditions 
[t,x]=ode23(‘penddiff’,t0,tf,x0); 

%plot(t,x(:,3)) 

plot(t,x) 
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PENDDIFF.M 


function xdot=penddiff(t,x) 
xdot=zeros(4,l); 

xdot(l)=x(2); 

xdot(3)=x(4); 

xdot(2)=9/4*sin(x(3))/(2*cos(x(3)) A 2-27)*x(4) A 2+5/2*cos(x(3))/(2*cos(x(3)) A 2-27)*x(4)+l/16* 

(-1568*cos(x(3))*sin(x(3))+45*x(2))/(2*cos(x(3)) A 2-27); 

xdot(4)=2*cos(x(3))*sin(x(3))/(2*cos(x(3)) A 2-27)*x(4) A 2+30/(2*cos(x(3)) A 2-27)*x(4)+l/ 

2*(5*cos(x(3))*x(2)-2352*sin(x(3)))/(2*cos(x(3)) A 2-27); 


16 



Belief Belief 


APPENDIX B 

MEMBERSHIP FUNCTIONS FOR SINGLE PENDULUM 



Variable Theta membership functions 

The membership functions for this variable are: 

Membership function NB: Points list: -128, 1 -80,1 -50,0 
Membership function NS: Points list: -25.18867925,0 -10, 1 0 ,0 
Membership function Z: Points list: -5,0 0,1 5, 0 
Membership function PS: Points list: 0,0 10, 1 25. 18867925,0 
Membership function PB: Points list: 50,0 80, 1 80, 1 127, 1 
Membership function NU: Points list: -60,0-30, 1-14.38679245,0 
Membership function PU: Points list: 15.58962264,0 30,1 60,0 



Variable dTheta membership functions 

The membership functions for this variable are: 


Membership function NB: Points list: -128, 1 -50,1 -20,0 
Membership function NS: Points list: -40,0-20,1 0,0 
Membership function Z: Points list: -20,0 -0.5,1 20,0 
Membership function PS: Points list: 0,0 20,1 40,0 
Membership function PB: Points list: 20,0 50,1 127,1 
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Variable Mrate membership functions 

The membership functions for this variable are: 


Membership function NB: Points list: -128, 1 -10,1 0,0 
Membership function Z: Points List: -4.716850829,0 0, 1 
5.069060773,0.002673796791 
Membership function PB: Points list: 0.0 10,1 127,1 
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Variable Motor membership functions 

The membership functions for this variable are: 


Membership function NB: Points list: -128, 1 -100,1 -50.0 
Membership function NS: Points list: -100,0-50,1 -0.5,0 
Membership function Z: Points list: -50,0 -0.5, 1 50.0 
Membership function PS: Points list: -0.5,0 50,1 100,0 
Membership function PB: Points list: 50,0 100,1 127,1 
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APPENDIX C 

TOGAI AND HANDWRITTEN CODE FOR SINGLE PENDULUM 


* .EXPORT Mpos 

* .EXPORT _Mrate 

* .EXPORT Theta 

* .EXPORT dTheta 

* .EXPORT _Motor 

* .EXPORT PENDULUM 1 

* .EXPORT _PENDULUM 1 Jnit 
; Input VARs 

DEFSEG ramvars,START=$a 
SEG ramvars 

ORG $a 

PENDULUM lvar: 

_Mpos: 

RMB 1 

_Mrate: 

RMB 1 

_Theta: 

RMB 1 

dTheta: 

RMB 1 

; Output VARs 
_Motor: 

RMB 1 

; Hidden VARs 
PENDULUMltemp: 

; Input MEMBER temps 
; MEMBER Z(Theta) 

RMB 1 

; MEMBER Z(dTheta) 

RMB 1 

; MEMBER NS(Theta) 

RMB 1 

; MEMBER PS(Theta) 

RMB 1 

; MEMBER PB(dTheta) 

RMB 1 

; MEMBER NU(Theta) 

RMB 1 
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; MEMBER PS(dTheta) 

RMB 1 

; MEMBER NS(dTheta) 

RMB 1 

; MEMBER NB(dTheta) 

RMB 1 

; MEMBER PU(Theta) 

RMB 1 

; MEMBER NB(Theta) 

RMB 1 

; MEMBER PB(Theta) 

RMB 1 

; Output VAR temps 
RMB 7 

DEFSEG romvars,START =$Oe5 
SEG romvars 

ORG $f3e5 

PENDULUM 1: 


FDB 

PENDULUM l_var 

FDB 

PENDULUM l_temp 

FDB 

PENDULUM linmbf 

FDB 

PENDULUM l_outmbf 

FDB 

PENDULUM lcode 

* .PAGE 

; Input MEMBER table 

PENDULUM l_inmbf : 

FDB 

plZTheta 

FDB 

plZdTheta 

FDB 

plPMpos 

FDB 

plNSTheta 

FDB 

plZMpos 

FDB 

plPBMrate 

FDB 

plZMrate 

FDB 

plNMpos 

FDB 

plPSTheta 

FDB 

plNBMrate 

FDB 

plPBdTheta 

FDB 

plNUTheta 

FDB 

plPSdTheta 

FDB 

plNSdTheta 

FDB 

plNBdTheta 

FDB 

plPUTheta 

FDB 

plNBTheta 

FDB 

plPBTheta 


20 



; MEMBER Z(Theta) 
plZTheta: 


FCB 

-5,0 

FCB 

0,255 

FCB 

5,0 

FCB 

127,0 

; MEMBER Z(dTheta) 

plZdTheta: 


FCB 

-20,0 

FCB 

-1,255 

FCB 

20,0 

FCB 

127,0 

; MEMBER P(Mpos) 

plPMpos: 


FCB 

-1,0 

FCB 

20,255 

FCB 

127,255 

; MEMBER NS(Theta) 

plNSTheta: 


FCB 

-26,0 

FCB 

-10,255 

FCB 

0,0 

FCB 

127,0 

; MEMBER Z(Mpos) 

plZMpos: 


FCB 

-10,0 

FCB 

-1,255 

FCB 

10,0 

FCB 

127,0 

; MEMBER PB(Mrate) 

plPBMrate: 


FCB 

0,0 

FCB 

10,255 

FCB 

127,255 

; MEMBER Z(Mrate) 

plZMrate: 


FCB 

-5,0 

FCB 

0,255 

FCB 

5,0 

FCB 

127,0 

; MEMBER N(Mpos) 

plNMpos: 


FCB 

-20,255 

FCB 

-1,0 

FCB 

127,0 
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; MEMBER PS(Theta) 

plPSTheta: 


FCB 

0,0 

FCB 

10,255 

FCB 

25,0 

FCB 

127,0 

; MEMBER NB(Mrate) 

pINBMrate: 


FCB 

-10,255 

FCB 

0,0 

FCB 

127,0 

; MEMBER PB(dTheta) 

plPBdTheta: 


FCB 

20,0 

FCB 

50,255 

FCB 

127,255 

; MEMBER NU(Theta) 

plNUTheta: 


FCB 

-60,0 

FCB 

-30,255 

FCB 

-15,0 

FCB 

127,0 

; MEMBER PS(dTheta) 

plPSdTheta: 


FCB 

0,0 

FCB 

20,255 

FCB 

40,0 

FCB 

127,0 

; MEMBER NS(dTheta) 

plNSdTheta: 


FCB 

-40,0 

FCB 

-20,255 

FCB 

0,0 

FCB 

127,0 

; MEMBER NB(dTheta) 

plNBdTheta: 


FCB 

-50,255 

FCB 

-20,0 

FCB 

127,0 

; MEMBER PU(Theta) 

plPUTheta: 


FCB 

15,0 

FCB 

30,255 

FCB 

60,0 

FCB 

127,0 
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; MEMBER 

NB(Theta) 

plNBTheta: 

FCB 

-80,255 

FCB 

-50,0 

FCB 

127,0 

; MEMBER 

PB(Theta) 

plPBTheta: 

FCB 

50,0 

FCB 

80,255 

FCB 

80,255 

FCB 

127,255 

* .PAGE 


; Output MEMBER table 

PENDULUM l_outmbf: 

; MEMBER Z(Motor) 

; M 6391.67 A 50 H 1 
FCB $23, $06 
FCB $C4,$10,$03 
; MEMBER PB(Motor) 

; M 11803.8 A 52 H 1 
FCB $62, $06 
FCB $46,$A9,$05 
; MEMBER PS(Motor) 

; M 8936.13 A 50.25 H 1 
FCB $2B,$06 
FCB $2D,$49,$04 
; MEMBER NB(Motor) 

; M 1508.67 A 53 H 1 
FCB $81, $06 
FCB $3B,$B9,$00 
; MEMBER NS(Motor) 

; M 3872.21 A 49.75 H 1 
FCB $1B,$06 
FCB $6D,$DB,$01 
* .PAGE 

; Knowledge base code 
; PROJECT PENDULUM 1 
PENDULUM lcode : 

; MEMBER Z(Theta) 

FCB $01,$00,$02 
FCB $10, $00 
; MEMBER Z(dTheta) 

FCB $01, $02, $03 
FCB $10, $01 
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; MEMBER NS(Theta) 

FCB $01, $06, $02 

FCB $10, $02 

; MEMBER PS(Theta) 

FCB $01,$10,$02 

FCB $10, $03 

; MEMBER PB(dTheta) 

FCB $01,$14,$03 

FCB $10,$04 

; MEMBER NU(Theta) 

FCB $01,$16,$02 

FCB $10, $05 

; MEMBER PS(dTheta) 

FCB $01, $18, $03 

FCB $10, $06 

; MEMBER NS(dTheta) 

FCB $01,$1A,$03 

FCB $10, $07 

; MEMBER NB(dTheta) 
FCB $01,$1C,$03 

FCB $10, $08 

; MEMBER PU(Theta) 

FCB $01,$1E,$02 

FCB $10, $09 

; MEMBER NB(Theta) 

FCB $01, $20, $02 

FCB $10,$0A 

; MEMBER PB(Theta) 

FCB $01,$22,$02 

FCB $10,$0B 

; FUZZY Pendulum_rules 
; VAR Motor 

FCB $12,$0C 

; RULE RuleOOOl 
FCB $0E,$00 

FCB $0E,$01 

FCB $05 

FCB $0B,$00,$0C 

; RULE Rule0053 

FCB $01, $04, $00 

FCB $0E,$02 

FCB $05 

FCB $0B,$05,$0C 

; RULE Rule0052 

FCB $01, $08, $00 
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FCB $0E,$OO 

FCB $05 

FCB $0B,$00,$0C 

; RULE Rule0046 

FCB $01,$0A,$01 

FCB $0E,$00 

FCB $05 

FCB $0B,$0A,$0C 

; RULE Rule0055 

FCB $01,$0C,$01 

FCB $0E,$00 

FCB $05 

FCB $0B,$00,$0C 

; RULE Rule0057 

FCB $01,$0E,$00 

FCB $0E,$03 

FCB $05 

FCB $0B,$0F,$0C 

; RULE Rule0047 

FCB $01, $12, $01 

FCB $0E,$00 

FCB $05 

FCB $0B,$14,$0C 

; RULE Rule0033 
FCB $0E,$04 

FCB $0E,$05 

FCB $05 

FCB $0B,$00,$0C 

; RULE Rule0034 
FCB $0E,$06 

FCB $0E,$05 

FCB $05 

FCB $0B,$00,$0C 

; RULE Rule0035 
FCB $0E,$01 

FCB $0E,$05 

FCB $05 

FCB $0B,$00,$0C 

; RULE Rule0036 
FCB $0E,$07 

FCB $0E,$05 

FCB $05 

FCB $0B,$14,$0C 

; RULE Rule0037 
FCB $0E,$08 
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FCB $0E,$05 

FCB $05 

FCB $0B,$0F,$0C 

; RULE Rule0038 
FCB $0E,$04 

FCB $0E,$09 

FCB $05 

FCB $0B,$05,$0C 

; RULE Rule0039 
FCB $0E,$06 

FCB $0E,$09 

FCB $05 

FCB $0B,$0A,$0C 

; RULE Rule0040 
FCB $0E,$01 

FCB $0E,$09 

FCB $05 

FCB $0B,$00,$0C 

; RULE Rule0041 
FCB $0E,$07 

FCB $0E,$09 

FCB $05 

FCB $0B,$00,$0C 

; RULE Rule0042 
FCB $0E,$08 

FCB $0E,$09 

FCB $05 

FCB $0B,$00,$0C 

; RULE RuleOOlO 
FCB $0E,$O0 

FCB $0E,$08 

FCB $05 

FCB $0B,$05,$0C 

; RULE RuleOOll 
FCB $0E,$00 

FCB $0E,$07 

FCB $05 

FCB $0B,$0A,$0C 

; RULE Rule0012 
FCB $0E,$03 

FCB $0E,$08 

FCB $05 

FCB $0B,$05,$0C 

; RULE Rule0013 
FCB $0E,$03 
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FCB $0E,$07 

FCB $05 

FCB $0B,$0O,$OC 

; RULE Rule0002 
FCB $0E,$02 

FCB $0E,$07 

FCB $05 

FCB $0B,$05,$0C 

; RULE Rule0008 
FCB $0E,$02 

FCB $0E,$01 

FCB $05 

FCB $0B,$0A,$0C 

; RULE Rule0003 
FCB $0E,$03 

FCB $0E,$06 

FCB $05 

FCB $0B,$0F,$0C 

; RULE Rule0009 
FCB $0E,$03 

FCB $0E,$01 

FCB $05 

FCB $0B,$14,$0C 

; RULE Rule0006 
FCB $0E,$02 

FCB $0E,$08 

FCB $05 

FCB $OB,$05,$0C 

; RULE Rule0007 
FCB $0E,$03 

FCB $0E,$04 

FCB $05 

FCB $0B,$0F,$0C 

; RULE Rule0014 
FCB $0E,$02 

FCB $0E,$06 

FCB $05 

FCB $0B,$00,$0C 

; RULE Rule0015 
FCB $0E,$02 

FCB $0E,$04 

FCB $05 

FCB $0B,$0F,$0C 

; RULE Rule0016 
FCB $0E,$00 
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FCB $0E,$06 

FCB $05 

FCB $0B,$14,$0C 

; RULE Rule0017 
FCB $0E,$00 

FCB $0E,$04 

FCB $05 

FCB $0B,$0F,$0C 

; RULE Rule0018 
FCB $0E,$08 

FCB $0E,$0A 

FCB $05 

FCB $OB,$OO,$0C 

; RULE Rule0022 
FCB $0E,$08 

FCB $0E,$0B 

FCB $05 

FCB $0B,$05,$0C 

; RULE Rule0024 
FCB $0E,$07 

FCB $0E,$0A 

FCB $05 

FCB $OB,$00,$OC 

; RULE Rule0025 
FCB $0E,$01 

FCB $0E,$0A 

FCB $05 

FCB $0B,$0A,$0C 

; RULE Rule0026 
FCB $0E,$06 

FCB $0E,$0A 

FCB $05 

FCB $0B,$14,$0C 

; RULE Rule0027 
FCB $0E,$04 

FCB $0E,$0A 

FCB $05 

FCB $0B,$0F,$0C 

; RULE Rule0029 
FCB $0E,$04 

FCB $0E,$0B 

FCB $05 

FCB $0B,$00,$0C 

; RULE Rule0030 
FCB $0E,$06 
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FCB $OE,$OB 

FCB $05 

FCB $0B,$00,$0C 

; RULE Rule0031 
FCB $0E,$01 

FCB $0E,$0B 

FCB $05 

FCB $0B,$14,$0C 

; RULE Rule0032 
FCB $0E,$07 

FCB $0E,$0B 

FCB $05 

FCB $0B,$0A,$0C 

FCB $0A,$0C,$04 

FCB $16, $4, $80 
FCB $00 

_PENDULUMl_init: 

LDX #_PENDULUM 1 

RTS 

OPTIONS 

END 

PROJECT PENDULUM 1 
OPTIONS 

DESCRIPTION=”This is a single inverted pendulum” 

GRIDSPACE=0.5,0 

GRIDSHOW=”ON” 

GRIDSNAP=”ON” 

ICONSCALE=l 

NORMALSIZE=”ON” 

REDUCETOFIT=”OFF” 

SHOWTOOLS=”ON” 

VIEWORIGIN=-0. 1,-0. 1 
VIEW SC ALE= 1 
END 


VAR Theta 


OPTIONS 
ICONCOLOR=0 
ICONPOS= 1.5,2 
GRIDSPACE=0,0 
GRIDSHOW=”ON” 
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GRIDSNAP=”OFF” 

END 

TYPE signed byte 
DEFAULT 0 

MEMBER NB 

OPTIONS 

ICONCOLOR=I 

END 

POINTS -128,1 -80,1 -50,0 
END 

MEMBER NS 

OPTIONS 

ICONCOLOR=2 

END 

POINTS -25.18867925,0 -10,1 0,0 
END 

MEMBER Z 

OPTIONS 

ICONCOLOR=3 

END 

POINTS -5,0 0,1 5,0 
END 

MEMBER PS 

OPTIONS 

ICONCOLOR=4 

END 

POINTS 0,0 10,1 25.18867925,0 
END 

MEMBER PB 

OPTIONS 

ICONCOLOR=5 

END 

POINTS 50,0 80,1 80,1 127,1 
END 


30 



MEMBER NU 


OPTIONS 

ICONCOLOR=5 

OUTPUTSCOPE=”PRIVATE” 

END 

POINTS -60,0 -30,1 -14.38679245,0 
END 

MEMBER PU 

OPTIONS 

ICONCOLOR=6 

OUTPUTSCOPE=”PRIVATE” 

END 

POINTS 15.58962264,0 30,1 60,0 

END 

END 

VAR dTheta 

OPTIONS 

ICONCOLOR=0 

ICONPOS=1.5,3.5 

GRIDSPACE=0,0 

GRIDSHOW=”ON” 

GRIDSNAP=”ON” 

END 

TYPE signed byte 

MEMBER NB 

OPTIONS 
ICONCOLOR= 1 
END 

POINTS -128,1 -50,1 -20,0 
END 

MEMBER NS 

OPTIONS 

ICONCOLOR=2 

END 

POINTS -40,0 -20,1 0,0 
END 



MEMBER Z 


OPTIONS 

ICONCOLOR=3 

END 

POINTS -20,0 -0.5,1 20,0 
END 

MEMBER PS 

OPTIONS 

ICONCOLOR=4 

END 

POINTS 0,0 20,1 40,0 
END 

MEMBER PB 

OPTIONS 

ICONCOLOR=5 

END 

POINTS 20,0 50,1 127,1 

END 

END 


VAR Motor 

OPTIONS 

ICONCOLOR=0 

ICONPOS=5. 5,2.5 

GRIDSPACE=0,0 

GRIDSHOW=”ON” 

GRIDSNAP=”ON” 

END 

TYPE signed byte 

MEMBER NB 

OPTIONS 
ICONCOLOR= 1 
END 

POINTS -128,1 -100,1 -50,0 
END 

MEMBER NS 
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OPTIONS 

ICONCOLOR=2 

END 

POINTS -100,0 -50,1 -0.5,0 
END 

MEMBER Z 

OPTIONS 

ICONCOLOR=3 

END 

POINTS -50,0 -0.5,1 50,0 
END 

MEMBER PS 

OPTIONS 

ICONCOLOR=4 

END 

POINTS -0.5,0 50,1 100,0 
END 

MEMBER PB 

OPTIONS 

ICONCOLOR=5 

END 

POINTS 50,0 100,1 127,1 

END 

END 


VAR Mrate 


OPTIONS 

ICONCOLOR=0 

ICONPOS=3,4 

GRIDSPACE=0,0 

GRIDSHOW=”ON” 

GRIDSNAP=”ON” 

END 

TYPE signed byte 

MEMBER NB 

OPTIONS 

ICONCOLOR=l 
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END 

POINTS -128,1 -10,1 0,0 
END 

MEMBER Z 

OPTIONS 

ICONCOLOR=3 

END 

POINTS -4.716850829,0 0,1 5.069060773,0.002673796791 
END 

MEMBER PB 

OPTIONS 

ICONCOLOR=5 

END 

POINTS 0,0 10,1 127,1 

END 

END 


FUZZY Pendulum_rules 

OPTIONS 
ICONCOLOR=0 
ICONPOS=3 .5 ,2 .5 
OUTPUTSCOPE=”PRIVATE” 

END 

RULE RuleOOOl 

OPTIONS 

ENABLE=”ON” 

END 

IF (Theta IS Z) AND (dTheta IS Z) THEN 

Motor=Z 

END 

RULE Rule0050 

OPTIONS 

ENABLE=”ON” 

END 
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IF (Mrate IS Z) AND (Theta IS Z) THEN 

Motor=Z 

END 

RULE Rule0046 

OPTIONS 

ENABLE=”ON” 

END 

IF (Mrate IS PB) AND (Theta IS Z) THEN 

Motor=PS 

END 

RULE Rule0047 

OPTIONS 

ENABLE=”ON” 

END 

IF (Mrate IS NB) AND (Theta IS Z) THEN 

Motor=NS 

END 

RULE Rule0033 

OPTIONS 

ENABLE=”ON” 

END 

IF (dTheta IS PB) AND (Theta IS NU) THEN 

Motor=Z 

END 

RULE Rule0034 

OPTIONS 

ENABLE=”ON” 

END 

IF (dTheta IS PS) AND (Theta IS NU) THEN 

Motor=Z 

END 



RULE Rule0035 


OPTIONS 

ENABLE=”ON” 

END 

IF (dTheta IS Z) AND (Theta IS NU) THEN 

Motor=Z 

END 

RULE Rule0036 

OPTIONS 

ENABLE=”ON” 

END 

IF (dTheta IS NS) AND (Theta IS NU) THEN 

Motor=NS 

END 

RULE Rule0037 

OPTIONS 

ENABLE=”ON” 

END 

IF (dTheta IS NB) AND (Theta IS NU) THEN 

Motor=NB 

END 

RULE Rule0038 

OPTIONS 

ENABLE=”ON” 

END 

IF (dTheta IS PB) AND (Theta IS PU) THEN 

Motor=PB 

END 

RULE Rule0039 

OPTIONS 

ENABLE=”ON” 

END 
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IF (dTheta IS PS) AND (Theta IS PU) THEN 

Motor=PS 

END 

RULE Rule0040 

OPTIONS 

ENABLE=”ON” 

END 

IF (dTheta IS Z) AND (Theta IS PU) THEN 

Motor=Z 

END 

RULE Rule0041 

OPTIONS 

ENABLE=”ON” 

END 

IF (dTheta IS NS) AND (Theta IS PU) THEN 

Motor=Z 

END 

RULE Rule0042 

OPTIONS 

ENABLE=”ON” 

END 

IF (dTheta IS NB) AND (Theta IS PU) THEN 

Motor=Z 

END 

RULE RuleOOlO 

OPTIONS 

ENABLE=”ON” 

END 

IF (Theta IS Z) AND (dTheta IS NB) THEN 

Motor=PB 

END 
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RULE RuleOOll 


OPTIONS 

ENABLE=”ON” 

END 

IF (Theta IS Z) AND (dTheta IS NS) THEN 

Motor=PS 

END 

RULE Rule0012 

OPTIONS 

ENABLE=”ON” 

END 

IF (Theta IS PS) AND (dTheta IS NB) THEN 

Motor=PB 

END 

RULE Rule0013 

OPTIONS 

ENABLE=”ON” 

END 

IF (Theta IS PS) AND (dTheta IS NS) THEN 

Motor=Z 

END 

RULE Rule0002 

OPTIONS 

ENABLE=”ON” 

END 

IF (Theta IS NS) AND (dTheta IS NS) THEN 

Motor=PB 

END 

RULE Rule0008 

OPTIONS 

ENABLE=”ON” 

END 
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IF (Theta IS NS) AND (dTheta IS Z) THEN 

Motor=PS 

END 

RULE Rule0003 

OPTIONS 

ENABLE=”ON” 

END 

IF (Theta IS PS) AND (dTheta IS PS) THEN 

Motor=NB 

END 

RULE Rule0009 

OPTIONS 

ENABLE=”ON” 

END 

IF (Theta IS PS) AND (dTheta IS Z) THEN 

Motor=NS 

END 

RULE Rule0006 

OPTIONS 

ENABLE=”ON” 

END 

IF (Theta IS NS) AND (dTheta IS NB) THEN 

Motor=PB 

END 

RULE Rule0007 

OPTIONS 

ENABLE=”ON” 

END 

IF (Theta IS PS) AND (dTheta IS PB) THEN 

Motor=NB 

END 
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RULE Rule0014 


OPTIONS 

ENABLE=”ON” 

END 

IF (Theta IS NS) AND (dTheta IS PS) THEN 

Motor=Z 

END 

RULE Rule0015 

OPTIONS 

ENABLE=”ON” 

END 

IF (Theta IS NS) AND (dTheta IS PB) THEN 

Motor=NB 

END 

RULE Rule0016 

OPTIONS 

ENABLE=”ON” 

END 

IF (Theta IS Z) AND (dTheta IS PS) THEN 

Motor=NS 

END 

RULE Rule0017 

OPTIONS 

ENABLE=”ON” 

END 

IF (Theta IS Z) AND (dTheta IS PB) THEN 

Motor=NB 

END 

RULE Rule0018 

OPTIONS 

ENABLE=”ON” 

END 
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IF (dTheta IS NB) AND (Theta IS NB) THEN 

Motor=Z 

END 

RULE Rule0022 

OPTIONS 

ENABLE=”ON” 

END 

IF (dTheta IS NB) AND (Theta IS PB) THEN 

Motor=PB 

END 

RULE Rule0024 

OPTIONS 

ENABLE=”ON” 

END 

IF (dTheta IS NS) AND (Theta IS NB) THEN 

Motor=Z 

END 

RULE Rule0025 

OPTIONS 

ENABLE=”ON” 

END 

IF (dTheta IS Z) AND (Theta IS NB) THEN 

Motor=PS 

END 

RULE Rule0026 

OPTIONS 

ENABLE=”ON” 

END 

IF (dTheta IS PS) AND (Theta IS NB) THEN 

Motor=NS 

END 



RULE Rule0027 


OPTIONS 

ENABLE=”ON” 

END 

IF (dTheta IS PB) AND (Theta IS NB) THEN 

Motor=NB 

END 

RULE Rule0029 

OPTIONS 

ENABLE=”ON” 

END 

IF (dTheta IS PB) AND (Theta IS PB) THEN 

Motor =Z 

END 

RULE Rule0030 

OPTIONS 

ENABLE=”ON” 

END 

IF (dTheta IS PS) AND (Theta IS PB) THEN 

Motor=Z 

END 

RULE Rule0031 

OPTIONS 

ENABLE=”ON” 

END 

IF (dTheta IS Z) AND (Theta IS PB) THEN 

Motor=NS 

END 

RULE Rule0032 

OPTIONS 

ENABLE=”ON” 

END 
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IF (dTheta IS NS) AND (Theta IS PB) THEN 

Motor=PS 

END 

END 

CONNECT 
FROM Theta 
TO Pendulum_rules 
END 

CONNECT 
FROM dTheta 
TO Pendulum_rules 
END 

CONNECT 

FROM Pendulum_rules 

TO Motor 

END 

CONNECT 
FROM Mrate 
TO Pendulum_rules 
END 
END 

SIMULATE SimulationOOOO 

OPTIONS 

DURATION=6 

SAMPLETIME=0.001 

END 

CHART ChartOOOO 
HCHART=”time” 

VCHART=”Motor” 

VCHART=”x3" 

TTTLE=”Single Pendulum” 

HTITLE=”time, seconds” 

VTITLE=”angle, radians” 

HMAX=”6" 

HMIN=”0" 

HTICK=”0.5" 

VMAX=”1" 

VMIN=”-1" 
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VTICK=”0.1" 

END 

CSPLOT CSPlotOOOO 

XVARNAME=”Theta” 

YVARNAME=”dTheta” 

BIND=”Theta=- 1 28 " 

BIND=”dTheta=-128" 

END 

MODEL ModelOOOO 

OPTIONS 

DIFFEQ=”OFF” 

END 

#CODE 
pi=3. 1415926; 
dt=timestep; 
gain=10; 


xl=0; 

x2=0; 

x3=0; 

x4=0; 

pend_angle=x3; 

cpr=256/6.28; /* counts/rad */ 
cprps=256/20; /* counts/rad/s */ 
torpc=.67/256; /* torque/count, (Nm) */ 
cpmrps=256/3/10; /* counts per motor rad/s */ 
#END_CODE 

#CODE 

Motorf=floor(Motor+.5); 
tm=gain * torpc * Motorf ; 
if tm>.67 then tm=.67; end 
if tm<-.67 then tm=-.67; end 

s3=sin(x3); 

c3=cos(x3); 

den=2*c3*c3-27; 
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xld=x2; 

x3d=x4; 

x2d=l/32*(72*s3*x4*x4+40*c3*x4+45*x2-3136*c3*s3-45000*tm)/den; 

x4d=l/4*(8*c3*s3*x4*x4+60*x4-5000*c3*tm+5*c3*x2-4704*s3)/den; 


xl=xl+xld*dt; 

x2=x2+x2d*dt; 

x3=x3+x3d*dt; 

x4=x4+x4d*dt; 

motor_angle=xl; 
pend_angle=pend_angle+x3d *dt; 

if xl>pi then xl=xl-2*pi; end 
if xl<-pi then xl=xl+2*pi; end 
if x3>pi then x3=x3-2*pi; end 
if x3<-pi then x3=x3+2*pi; end 

Theta=x3*cpr; /* counts */ 
dThe ta=x4 * cprps ; 

Mrate=x2*cpmrps; 

if Theta>127 then Theta=127; end 
if Theta<-127 then Theta=-127; end 
if dTheta>127 then dTheta=127; end 
if dTheta<-127 then dTheta=-127; end 
if Mrate>127 then Mrate=127; end 
if Mrate<-127 then Mrate=-127; end 

Theta=floor(Theta+.5); 

dTheta=floor(dTheta+.5); 

Mrate=floor(Mrate+.5); 

#END_CODE 

END 

END 


;TEST PROGRAM FOR INVERTED ONE ARM PENDULUM USING THE 68HC811E2 

MICROCONTROLLER 

;BY TOM SUTHERLAND 

; 1/7/94 with mods through 7/8/94 
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.************************************************************************ 


;DEFINITIONS FOR REGISTER LOCATIONS 

;Purpose of initializing each of the registers is to be able to perform 

;bit manipulations on the registers without having to use indexed 

;addressing. 

ORIG INIT EQU $ 103D ;Location of INIT register before 

registers are remapped to page 0. 

;Location of registers after being remapped to page 0 


PORTA 

EOU 

$0000 

PIOC 

EQU 

$0002 

PORTC 

EQU 

$0003 

PORTB 

EQU 

$0004 

PORTCL 

EQU 

$0005 

DDRC 

EQU 

$0007 

PORTD 

EQU 

$0008 

DDRD 

EQU 

$0009 

PORTE 

EQU 

$000A 

CFORC 

EQU 

$000B 

OC1M 

EQU 

$000C 

OC1D 

EQU 

$000D 

TCNT 

EQU 

$000E 

TIC1 

EQU 

$0010 

TIC2 

EQU 

$0012 

TIC3 

EQU 

$0014 

TOC1 

EQU 

$0016 

TOC2 

EQU 

$0018 

TOC3 

EQU 

$001 A 

TOC4 

EQU 

$001C 

TI405 

EQU 

$001 E 

TCTL1 

EQU 

$0020 

TCTL2 

EQU 

$0021 

TMSK1 

EQU 

$0022 

TFLG1 

EQU 

$0023 

TMSK2 

EQU 

$0024 

TFLG2 

EQU 

$0025 

PACTL 

EQU 

$0026 

PACNT 

EQU 

$0027 

SPCR 

EQU 

$0028 

SPSR 

EQU 

$0029 

SPDR 

EQU 

$002A 

BAUD 

EQU 

$002B 


;I/0 Port A 

parallel I/O Control Register 
;I/0 Port C 
;Output Port B 
;Alternate Latched Port C 
;Data Direction for Port C 
;I/0 Port D 

;Data Direction for Port D 
;Input Port E 
;Compare Force Register 
;OCl Action Mask Register 
;OCl Action Data Register 
;Timer Counter Register (16 bits) 
;Input Capture 1 Register (16 bits) 
;Input Capture 2 Register (16 bits) 
;Input Capture 3 Register (16 bits) 
;Output Compare 1 Register (16 bits) 
;Output Compare 2 Register (16 bits) 
;Output Compare 3 Register (16 bits) 
;Output Compare 4 Register (16 bits) 
; Output Compare 5 / Input Capture 4 
;Timer Control Register 1 
;Timer Control Register 2 
;Timer Interrupt Mask Register 1 
;Timer Interrupt Flag Register 1 
;Timer Interrupt Mask Register 2 
;Timer Interrupt Flag Register 2 
;Pulse Accumulator Control Register 
;Pulse Accumulator Count Register 
;SPI Control Register 
;SPI Status Register 
;SPI Data Register 
;SCI Baud Rate Control 
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SCCR1 

EQU 

$002C 

SCCR2 

EQU 

S002D 

SCSR2 

EQU 

$002E 

SCDR 

EQU 

$002F 

ADCTL 

EQU 

$0030 

ADR1 

EQU 

$0031 

ADR2 

EQU 

$0032 

ADR3 

EQU 

$0033 

ADR4 

EQU 

$0034 

BPROT 

EQU 

$0035 

OPTION 

EQU 

$0039 

COPRST 

EQU 

$003A 

PPROG 

EQU 

$003 B 

HPRIO 

EQU 

S003C 

INIT 

EQU 

$003 D 

TEST1 

EQU 

$003E 

CONFIG 

EQU 

$003F 


;SCI Control Register 1 
;SCI Control Register 2 
;SCI Status Register 
;SCI Data Register 
;A/D Control Register 
;A/D Result Register 1 
;A/D Result Register 2 
;A/D Result Register 3 
;A/D Result Register 4 
;EEPROM Block Protect Register 
;System Configuration Options 
;Arm/Reset COP Timer Circuit 
;EEPROM Program Control Register 
;High Priority I-bit interrupt & misc 
;Ram and I/O Mapping Register 
;Factory TEST Control Register 
;COP, ROM, and EEPROM Enables 


DEFINITIONS FOR RAM LOCATIONS ( 192 bytes available ) 
DEFSEG ramst, START=$40 


SEG ramst 

ORG $0040 ;Point to the first byte of RAM 

POSITION: RMB 2 POSITION OF FREE ARM 1. 

MSB POS: RMB 1 ;8-BIT POSITION SAVED FROM VELOCITY. 

OLD POS: RMB 2 ;SAVE LAST POSITION TO GET RATE. 

TIMER: RMB 3 INCREMENTED EVERY X ms 

DEFINITIONS FOR EEPROM LOCATIONS 

;These vectors point to the corresponding locations in memory. 


EEPROMSTART 

EQU 

$F800 

;Location of EEPROM in memory 

VECTORTICl 

EQU 

$FFEE 

; Location of vector for TIC1 

VECTORTIC2 

EQU 

$FFEC 

;Location of vector for TIC2 

VECTORTIC3 

EQU 

$FFEA 

;Location of vector for TIC3 

VECTOR_TOC2 

EQU 

$FFE6 

;Location of vector for TOC2 

VECTORRESET 

EQU 

$FFFE 

;Location of vector for reset 
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.************************************************************************ 

* 

;PROGRAM CODE 


MFPL EQU $f800 ;Location of run-time module 

DEFSEG stkseg, START=$ff 
SEG stkseg 
ORG $ff 

STAKDS $1 

DEFSEG entseg, START=$ffe4 
SEG entseg 
ORG $ffe4 

FDB ENTRY 

DEFSEG INTTOC2, START=VECTOR_TOC2 
SEG INTTOC2 
ORG VECTOR_TOC2 
FDB MAIN ROUTINE 

DEFSEG INTRESET, START = VECTOR RESET 

SEG INTRESET 

ORG VECTO R_RESET 

FDB START 

DEFSEG rom, START=$f800 
SEG rom 

ORG $f800 ;Set PC to start of EEPROM 


;Initialization code. 

START: CLR ORIGJNIT ;Remap both the registers and 

;the ram to page 0. This reduces 
;the available ram to 192 bytes, 

;but allows use of the bit 
manipulation and test instructions 
;with direct addressing. This 
remapping can only be done during 
;the first 64 clock cycles after 
;reset. 
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ENTRY: LDS 

#$OOFF 

initialize the stack pointer. 

LDAA 

#$30 

STAA 

BAUD 

;SET SERIAL TO 9600 BAUD 

LDAA 

#$0C 

;SET BITS TO ENABLE RECV. AND TRANSMIT. 

STAA 

SCCR2 

;ENABLE SERIAL I/O. 

LDAA 

#$00 

;SET PORT-C AS INPUTS. 

STAA 

DDRC 

. 64 66 
5 

LDAA 

#$80 

;SET INITIAL VALUE IN D/A PORT. 

JSR 

MOTORDRIVE 

. 66 66 66 
9 

;SET UP ADC REGISTERS. 

LDAA 

#$80 

;SET ADPU BIT, TO ENABLE CHARGE PUMP FOR ADC 

STAA 

OPTION 

. 66 66 
5 

LDAA 

#$20 

;SET BITS IN REG. ADCTL:SCAN=1,MULT=0 

STAA 

ADCTL 

, 66 66 
9 


CLRA 


INITIALIZE COUNT TO ZERO 

STAA 

TIMER 


STAA 

TIMER+1 


STAA 

TIMER+2 


BCLR 

TCTL1,$80 

;TOGGLE OUTPUT ON SUCCESSFUL COMPARE 

BSET 

TCTL1,$40 


LDD 

TCNT 

;ADD 60000 TO MAIN TIMER 

ADDD 

#60000 


STD 

TOC2 

;STORE VALUE IN COMPARATOR 

LDAA 

#$40 


STAA 

TFLG1 

;CLEAR TIMER FLAG 

BSET 

TMSK1,$40 

; TIMER OUTPUT COMPARE 2 

CLI 


;ENABLE INTERRUPTS 

HANG: BRA 

HANG 

;WAIT HERE FOR INTERRUPT 

MAINROUTINE: 


;THIS IS THE INTERRUPT SERVICE ROUTINE 

LDD 

TOC2 


ADDD 

#60000 

;ADD 30 ms TO TOC2 

STD 

TOC2 


LDD 

#1 

INCREMENT 24-BIT COUNT BY 1 

ADDD 

TIMER+1 


STD 

TIMER+1 
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LDAA#0 
ADCA TIMER 
STAA TIMER 

LD AA #$40 

STAA TFLG 1 ;CLEAR TOC2 FLAG 

LDAAADR1 ;8-BIT A/D OF MOTOR TACH. 

CLC ;REMOVE BIAS. 

SUBA #$7F 
JSR SND_WORD 

STAA Mrate ;S AVE FOR CONTROLLER. 

BGE HEEP 

COMA 

INCA 

LSRA 

LSRA 

; LSRA 

LSRA 

COMA 

INCA 

BRA HEEP1 

HEEP LSRA 

LSRA 

; LSRA 

LSRA 

HEEP1 ADDAMpos 

STAA_Mpos 
JSR SND_WORD 

JSR ARM_VELOCITY ;GET FREE ARM RATE (12-BIT) [SHML] 

STAA dTheta ;SAVE FOR CONTROLLER. 

; JSR SND_WORD 

LDD POSITION ;GET CURRENT POSITION. 

STD OLD POS ;SAVE FOR NEXT RATE CALC. 

LDAA #$80 CONSTANT TO MATCH MFPL INPUT SCALE 

CLC 

SUBA MSB POS ;8-BIT POSITION OF FREE ARM 
STAA Theta ;S AVE FOR CONTROLLER 

; JSR SND_WORD 

LDX #_PENDULUM1 ;MicroFPL NAME 

JSR MFPL DETERMINE ACTION THAT SHOULD BE TAKEN. 

LDAA #$7F CONSTANT TO SHIFT 0-255. 

CLC 

SUBA MOTOR ;DO THE SHIFT 
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; JSR SNDWORD 

CMPA#$40 ;LIMIT +SIDE. 

BHS SKIP3 ;GO CHECK NEGATIVE VOLTS. 

LDAA #$40 ;LIMIT TO +5 VOLTS. 

BRA SKIP ;CAN’T BE NEGATIVE. 

SKIP3: CMPA #$C0 ;LIMIT-SIDE. 

BLS SKIP ;NOT TOO NEGATIVE. 

LDAA #$C0 ;LIMIT TO -5 VOLTS. 

SKIP: JSR MOTORDRIVE ; COMMAND MOTOR. 

; JSR SND WORD 

LDAA #32 
JSR SND BYTE 

RTI ;RETURN FROM INTERRUPT 

;SUBROUTINE SECTION 

MOTOR DRIVE: COMA SUBROUTINE TO DRIVE MOTOR. 

INCA ;REVERSE SIGN OF DRIVE. 

STAA PORTB ;SEND 8 BIT DATA TO DIGITAL-TO- ANALOG CON 

VERTER. 

RTS ; RETURN FROM SUBROUTINE MOTOR_DRIVE. 

ARM VELOCITY: SUBROUTINE, DETERMINING FREE ARM’S VELOCITY. 

LDAB PORTD ;READ RESOLVER-TO-DIGITAL CONVERTERS BITS 
(LOW OF 12 BITS). 

LDAA PORTC ;READ RESOLVER-TO-DIGITAL CONVERTERS BITS 

(HIGH OF 12 BITS). 

STAA MSB POS SAVE CURRENT 8-BIT POSITION. 

LSLB SHIFT ONE BIT LEFT. 

LSLB ; “ “ 

LSRD SHIFT DOUBLE ACC‘ AB’ RIGHT. 

LSRD ; “ “ 

LSRD ; “ “ 

LSRD ; “ “ 

CLC ;CLEAR CARRY BIT. 

STD POSITION STORE THE FREE ARM’S CURRENT POSITION. 
SUBD OLD POS SUBTRACT POSITIONS. 

BMI NEG ;CHECK SIGN OF VELOCITY. 

POS: CPD #$7F SEE IF MAX POSITIVE EXCEEDED. 

BCS VEL1 ;NOT POSITIVE MAX YET. 

LDAB #$7F ;MAX VALUE OF 127. 

BRA VEL1 ; THROUGH WITH POSITIVE. 

NEG: CPD #$FF7F SEE IF MAX NEGATIVE EXCEEDED. 

BCC VEL1 ;DON’T LIMIT. 

LDAB #$F0 ;MAX VALUE OF -128. 
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VEL1: 


SND WORD: 


HEX_ASCII: 

HEXACHAR : 
SNDBYTE: 
SND_WAIT: 
RECV_BYTE: 


TBA ;PUT IN A-REGISTER. 

COMA 

INCA 

RTS ;RETURN. 


;CONVERT ACC‘A’ TO ASCII AND SEND TO SERIAL 
PORT. 


PSHA 

PSHA 

LSRA 

LSRA 

LSRA 

LSRA 

JSR HEX_ASCII 
JSR SND_BYTE 
PULA 

JSR HEX_ASCII 
JSR SND_BYTE 
PULA 
RTS 


;SAVE ORIGINAL DATA. 
;SAVE ORIGINAL DATA. 
;SHIFT HIGH NIBBLE RIGHT. 


;CONVERT HIGH NIBBLE TO ASCII. 

;SEND HIGH NIBBLE TO SERIAL PORT. 

;GET ORIGINAL DATA. 

;CONVERT LOW NIBBLE TO ASCII. 

;SEND LOW NIBBLE TO SERIAL PORT. 
;RESTORE ORIGINAL DATA. 

;RETURN FROM SND_WORD SUBROUTINE 


SUBROUTINE TO CONVERT FROM HEX TO ASCII. 
ANDA #S0F ;MASKOFF LOW NIBBLE. 

CMPA #$09 

BGT HEX A CHAR ;IF ITS GREATER THAN 9hex ITS A CHARACTER. 
ADDA #$30 ;ADD 30hex TO MAKE A NUMBER ASCII. 

RTS ;RETURN FROM HEX_ASCCI SUBR 

ADDA #$37 ;ADD 37hex TO NIBBLE. 

RTS ;RETURN FROM HEX_ASCII SUBROUTINE. 

SUBROUTINE TO SEND A BYTE TO SERIAL PORT. 
BRCLR SCSR2,$80,$ 

STAA SCDR 
BRCLR SCSR2,$80,$ 

RTS 

BRCLR SCSR2,$20,$ ;WAIT FOR CHARACTER IN BUFFER 
LDAA SCDR ;READ CHARACTER. 

RTS ;RETURN FROM SUBROUTINE RECV BYTE. 


rem c:\til\mfpl6811 -v c:\til\penddown 



copy onearm.asm+penddown.asm+tmpend temp.asm 
avmacll temp.asm >penddown.lst 

avlink penddown.mot = temp.obj, rtm6811.obj OF=MOT -PS(RTM_ROMSEG,f800h) - 
PS(rom,fddfh) -PS(RAMSEG,48h) -PS(RAMVARS,51H) 

AVLINK LOAD MAP 

For: Mixed Languages 


RELOCATED SEGMENTS - CLASS ‘M’ 


SEGMENT NAME 

START 

RAMSEG 

0000 

RAMVARS 

000a 

RAMST 

0040 

STKSEG 

OOff 

REGS 

1000 

RTMROMSEG 

f800 

ROMVARS 

fbe7 

ROM 

fddf 

ENTSEG 

ffe4 

INTTOC2 

ffe6 

INTRESET 

fffe 


ZERO LENGTH SEGMENTS 


STOP LENGTH ovl/cat def/undef 

0008 0009 Concat Defined 

OOlf 0016 Concat Defined 

0047 0008 Concat Defined 

OOff 0001 Concat Defined 

103f 0040 Overld Defined 

fbe6 03e7 Concat Defined 

fdde 01f8 Concat Defined 

fed8 OOfa Concat Defined 

ffe5 0002 Concat Defined 

ffe7 0002 Concat Defined 

ffff 0002 Concat Defined 


SEGMENT START 

PAGE0 0000 

CODE 0000 

DATA 0000 

No Transfer Address. 


pendulum.mot=temp.obj, (later, improved mapping) 
rtm6811.obj 
OF=MOT 

-PS(RTM_ROMSEG, F800H) 
-PS(ROM,FE47H) 

-PS(RAMSEG,48H) 

-PS(RAMVARS,51H) 

-PS(ROMVARS,FBE7H) 
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Object code in ASCII format as sent to the 6811 by Procom 

S 1 07FFE4FDE8FE280 A 
S105FFFEFDDF21 

S123FDDF7F103DCC0600FD00438E00FF8630972B860C972D860097078680BDFE858600B720 

S123FDFF004286809739862097304FB70045B70046B70047152080142040DC()EC3EA60DD18 

S 1 23FE 1 F 1 886409723 1 422400EDC 1 8C3EA60DD 1 8CC0001 F30046FD00468600B90045 B70029 

S123FE3F4586409723BDFE88B7000BFC0040FD004386809003B7000ACEFBE7BDF800F600A4 

S123FE5F0C58588680B0000C434CBDFE8520BA963185802709847F448A80B7004239438412 

S123FE7F7F44B7004239970439D608960358585858040404040CFD0040B3004381002B03C1 

S123FE9F17200317434C39363644444444BDFEBBBDFEC732BDFEBBBDFEC73239840F810900 

S 1 1 DFEBF2E038B30398B3739 1 32E80FC972F1 32E80FC39132E20FC962F393 1 
S 1 23FBE7000 AOOODFBF 1 FC65FC7EFC09FC1 1 FC1 9FC1FFC27FC2FFC37FC3DFC45FC4DFC5 743 

S123FC07FC5DF600FFFF0A007FO0EC00FFFF14O07F0O140O32FF7FFFC4O0D8FFECO07FO0BD 
S123FC27000014FF28007FOOD800ECFF00007FOOCEFFEC007F00140028FF3C007FOOFF0090 
S 1 23FC47 14FF 1 E007FOOE200ECFFECFFFF007FOOBOFFC4007F003C005 0FF5 0FF7FFF300732 
SI 23FC673C97032707E42C029F07F9D8007 A079 AA1063 A072D05050 1 000010000 102011 08D 
S123FC8701010401100201060010030108011004010A011005010C011006010E001007019C 

S 1 23FC A7 1 000 1008011 200 1 0090 1 1 400 1 00 AO 1 1 600 1 OOB 120C0E000E0 1 050BOO0C0EO2OE0F 

S123FCC703O5OBOOOCOEO4OEO3O5OB0OOCOE01OEO3O5OBO0OCOEO5OEO3O50BO50COEO60E18 

S123FCE7O3O50B0A0C0E020E07050B0F0C0E040E0705OB140COEO10EO7050BO00COE050EC8 

S123FDO707050BOOOC0E060EO7O5OB0OOCOE0O0E06O5OBOFOCOEOOOEO5050B140C0EO80EAE 

S123FD2706050BOFOCOE080E05050BOOOCOE090E05050BOFOCOE090EO 1 050B 1 40C0E080E73 

S123FD4704050BOAOCOE080E01050B050COE090E06050BOFOCOE080E02050BOAOCOE090E61 

S123FD67O4050B0O0C0E090EO205OBOAOC0EOOOEO405OBO50COE0OOEO2O5OB0A0C0EO6OE64 

SI 23 FD 87 OAO 5 OBOOOCOEO 6 OEOBO 5 OBOFOCOEO 5 OEOAO 5 OBOOOCOEOIOEOAO 5 OBOOOCOEO 4 OE 3 O 

SI 23 FDA 7 OAO 5 OBOOOCOEO 2 OEOAO 5 OBOAOCOEO 2 OEOBO 5 OBOOOCOEO 4 OEOBO 5 OBOOOCOEOIOEIB 

S 1 1 BFDC70B05 OBOOOCOE050EOB05 0B000C0 A0C02 1 6028000CEFBE739 1 8 

S123F800FFOOOOEE08FF0002201301400128012301290131013601370138FE0002A600087B 

S 1 23F820FF00028 1 1 72D0220FECEF833 1 6 1 B 1 B 1 63 A6E007EF8787EF8897EF9 1 67EF95 97E 1 3 

S123F840F9867EF9BB7EF9C57EF9CD7EF9D47EF9E57EF9F07EFA727EFB0C7EFB2B7EFB4495 

SI 23F8607EFB4B7EFB647EFB6B7EFB7 1 7EFB977EFB A87EFBAE7EFBC8390 1 400 1 2801240 1 B3 

S123F88O29O1D7015C015A015A18FE0O0218E6001808FE0OOOEEO43AEE003CFEO0O0EE00DA 

S 1 23 F8 AO 1 8E600 18083 AA600 1 8FF000238 A1 002E06E60 1 377EF8 1 AE600F7 0004E60 1 F700B3 
S 1 23F8C0050808 A10027E A2EEEE60 1 F0000527E1 252 1 B000043DFD0006A600B00004B7001 2 

S123F8E008FE00068600F600088F028FFB0005377EF81A50B000043DFD0006A600B00004EF 
SI 23F900B70008FE00068600F600088F028F50FB0005377EF81 A1 8FE0002 1 8E600 1 808FE3 1 
S 1 23F9200000EE043 AEE003CFE0000EE00 1 8E600 1 8083 AA600 1 8FF00023 8 A 1 002206E60 182 

S123F940377EF81AE600F70004E601F700050808A10027EA22EE7EF8C918FE000218E600F1 

S 1 23F960 1 808FE0000EE043 AEEOO 1 8E600 18081 8FF0002 1 8FE0000 1 8EE00 1 83 A1 8E6003 A6E 
S 1 23F980A600367EF8 1 A1 8FE0002 1 8E6001 808FE0000EE043 A1 8E6001 808 18FF0002 1 8FE4 A 
S 1 23F9 AOOOOO 1 8EE00 1 83 A 1 8E6008600C 1 8025 0286FFE3008FA600367EF8 1A323311250100 
S 1 23F9C0 1 7367EF8 1 A3233 1 1 22F7 1 720F43233 1 B367EF81 A3233 1 B28082 A04867F200286BB 
S 1 23F9E080367EF8 1 A3233 1 B240286FF367EF8 1 AFE0000EE02 1 8FE0002 1 8E6001 808 1 8FF9 1 
S 1 23FA0000023 AA6068 1 002627 A6028 1 002621 09 A6068 1 0026 1 AA6028 1 0026 1 409 A6028 1 B 1 
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S 1 23FA20002625 1 8FE0002 18081 8FF00027EF8 1 AA6068 1 8024 1 2 A6028 1 80240C680 1 690206 
S 1 23FA4068046905 690620E88600E602C 10027FE3736A606E60538028F1 7FE0000EE00 1815 
S 1 23FA60FE0002 1 8E600 1 808 1 8FF00023 AA7007EF8 1 A8D6 A32361 8E6003DEB008900B700 1 5 
SI 23FA8004E7003236 1 8E60 1 3DEB0 1 8900FB00048900B70004E70 1 A602BB0004 A7023236BB 
S123FAA018E6023DEB038900B70004E703323618E6033DEB048900FB00048900B70004E7A1 
S123FAC0043218E6043DEB058900FB00048900B70004E705A606BB0004A7067EF81A18FE47 
S 1 23FAE00000 1 8EE06FE0002E60008FF0002 1 83 A1 8FF0006FE0000EE02 1 8FE0002 1 8E60094 
S123FB00180818FF00023A18FE0006398DD086003337EB008900E700168600EB018900E779 
S 1 23FB200 1 1 68600EB02E7 027EFA9EFE0000EE00 1 8FE0002 1 8E600 1 808 1 8FF00023 AA600 1 D 
S 1 23 FB40367EF8 1 AFEOOOOEEO220E5 FEOOOOEEOO 1 8FE0002 1 8E600 1 808 1 8FF00023 A3 2 A79 A 
S 123FB60007EF8 1 AFE0000EE0220E53243367EF8 1 AFE0000EE02 1 8FE0002 1 8E600 1 808 1887 
S 1 23FB80FF00023 A8600 A700A70 1 A702A703 A704 A705 A7067EF8 1 A1 8FE0002 1 8 A600 1 80874 
S 1 23FB A01 8FF0002367EF8 1 A3236367EF8 1 AFE0000EE001 8FE0002 1 8E600 1 808 1 8FF0002F9 
S 1 23FBC03 A323 6 A7007EF8 1 AFEOOOOEEOO 1 8FE0002 1 8E600 1 8083 AA600 1 8 AOOO 180818FF57 
S10AFBE00002A7007EF8 1 AE1 
S9030000FC 
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